import rospy

from robotiq_2f_gripper_control.msg import \
    _Robotiq2FGripper_robot_output as outputMsg

import roslib; roslib.load_manifest('robotiq_2f_gripper_control')


class GripperManager():
    def __init__(self, speed = 255, force = 50):
        self.__pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output , queue_size=1) 

        self.__speed = speed
        self.__force = force

        self.__command = outputMsg.Robotiq2FGripper_robot_output()

    def activate(self):
        # reset gripper
        rospy.loginfo('Resetting the gripper driver...')
        self.__command.rACT = 0
        self.__command.rGTO = 0
        self.__command.rATR = 0
        self.__command.rPR = 0
        self.__command.rSP = 0
        self.__command.rFR = 0
        self.__pub.publish(self.__command)
        rospy.sleep(5)

        # activate
        rospy.loginfo('Activating the gripper driver...')
        self.__command.rACT = 1
        self.__command.rGTO = 1
        self.__command.rATR = 0
        self.__command.rPR = 0
        self.__command.rSP  = self.__speed
        self.__command.rFR  = self.__force
        self.__pub.publish(self.__command)
        rospy.sleep(1)

        rospy.loginfo('Gripper initialized.')

    def open(self, speed = None, force = None):
        self.__command.rPR = 0
        self.__command.rATR = 0
        self.__command.rGTO = 1
        self.__command.rACT = 1
        self.__command.rSP  = speed if speed != None else self.__speed
        self.__command.rFR  = force if force != None else self.__force
        self.__pub.publish(self.__command)

    def close(self, speed = None, force = None):
        self.__command.rPR = 255
        self.__command.rATR = 0
        self.__command.rGTO = 1
        self.__command.rACT = 1
        self.__command.rSP  = speed if speed != None else self.__speed
        self.__command.rFR  = force if force != None else self.__force
        self.__pub.publish(self.__command)

if __name__ == "__main__":
    rospy.init_node('GRIPPER_MANAGER', anonymous=True)
    
    gm = GripperManager()

    # activate the gripper
    gm.activate()

    gm.open()
    rospy.loginfo('gripper opened.')
    rospy.sleep(2)
    gm.close()
    rospy.loginfo('gripper closed.')
    rospy.sleep(2)
    gm.open(speed=100)
    rospy.loginfo('gripper opened.')
    rospy.sleep(2)
    gm.close(speed=50)
    rospy.loginfo('gripper closed.')
    rospy.sleep(2)
    gm.open(force=50)
    rospy.loginfo('gripper opened.')
    rospy.sleep(2)
    gm.close(force=20)
    rospy.loginfo('gripper closed.')
    rospy.sleep(5)
